/*
 * EKF.cpp
 *
 *  Created on: May 26, 2011
 *      Author: gbower
 */

#include "EKF.h"
#include <iostream>
using namespace std;

const int ione = 1;
const double done = 1.0;
const double dzero = 0;


/**********************************
Initialize the filter
**********************************/
EKF::EKF(FileWriter *fwi) {

	// Model States:
	// Inertial positions: N, E, D
	// Inertial velocities: VN, VE, VD
	// Attitude quaternions: q0, q1, q2, q3
	// North and East Winds: WindN, WindE
	// Gyro biases: wbx, wby, wbz
	// Pressure biases: barob
	// Acceleration biases: abx, aby, abz

	// Initialize Covariance Matrix
	double P_Diag_Init[numStates] = {100,100,100,
		    10,10,10,
		    0.01,0.01,0.01,0.01,
		    4,4,0.0001,0.0001,0.0001,
		    400,0.001,0.001,0.001};


	for (int i = 0; i<numStates; i++) {
		for (int j = 0; j<numStates; j++) {
			if (i == j) {
				*(P+i+j*numStates) = P_Diag_Init[i];
			} else {
				*(P+i+j*numStates) = 0.0;
			}
		}
	}
	
	// Process noise covariance matrix (assumed diagonal)
	R = {0.1,0.1,0.1,0.1,0.1,0.1,
		    1e-8,1e-8,1e-8,1e-8,
		    1e-5,1e-5,1e-8,1e-8,1e-8,
		    50,1e-6,1e-6,1e-6};
		    
	// Measurement covariance matrices (assumed diagonal)
	// Baro pressure, Differential pressure, Mx, My, Mz, GPSN, GPSE, GPSD, GPSVN, GPSVE, GPSVD, ubody, vbody, wbody 
	//Q_IMU_GPS = {50.0,4.0,2.5e-5,2.5e-5,1.6e-5,16,16,36,0.1,0.1,0.25,0.1,0.1,0.1};
	Q_IMU_GPS = {50.0,4.0,1.0,1.0,1.0,25.0,25.0,36.0,0.5,0.5,1.0,0.3,0.3,0.3};
	Q_IMU = {50.0,4.0,1.0,1.0,1.0};
	Q_IMU_GPS_Sonar = {50.0,4.0,1.0,1.0,1.0,25.0,25.0,1e5,0.5,0.5,1.0,0.3,0.3,0.3,0.001};
	Q_IMU_Sonar = {50.0,4.0,1.0,1.0,1.0,0.001};
	
	// Nominal filter timestep
	dt = 0.01;

	fw = fwi;
}


/**********************************
What to do when data is received
from the autopilot. Run an
iteration of the fitler!
**********************************/
void EKF::AUPdataReceived(Communication &com) {
	
	// Determine packet type
	int flag = 1; // IMU update

	if (com.datInitReceived && !com.runEKF) {
		initializeState();
		com.runEKF = true;
		dt = 0.01;
		ts = com.timer.getTime();
		cout << "Initial GPS packet received.." << endl;
	} else if (com.runEKF && com.imuin) {
		dt = com.tIMU[1] - com.tIMU[0];

		/*
		 *   Nothing happens when a GPS packet is received. We always wait for the
		 *   next IMU packet before performing an update. So check to see if the
		 *   latest GPS time is between the current IMU time and the previous IMU
		 *   time. If so, perform the combined measurement update (flag = false).
		 */
		if ((com.tIMU[1] > com.tGPS[1]) && (com.tIMU[0] < com.tGPS[1])) {
			flag = 0; // GPS update
			if ((apdat.sonar_range < 7) && (apdat.sonar_range > 0.5)) {
				flag = 3; // GPS & sonar update
			}
		} else if ((apdat.sonar_range < 7.0) && (apdat.sonar_range > 0.5)) {
			flag = 2; // IMU & sonar update
		}


		// Initialize local variables
		int N = numStates;
		int M;
		double *xcurr = (double*)malloc(sizeof(double)*numStates);
		double *mubt = (double*)malloc(sizeof(double)*numStates);
		double *G = (double*)malloc(sizeof(double)*numStates*numStates);
		double *Sbart = (double*)malloc(sizeof(double)*numStates*numStates);
		double *H, *Kt, *zbt;
		if (flag == 1) { // IMU update
			M = imuSize;
			H  = (double*)malloc(sizeof(double)*N*M);
			Kt = (double*)malloc(sizeof(double)*N*M);
			zbt = (double*)malloc(sizeof(double)*M);
		} else if (flag == 0) { // IMU & GPS update
			M = imuSize+gpsSize;
			H = (double*)malloc(sizeof(double)*N*M);
			Kt = (double*)malloc(sizeof(double)*N*M);
			zbt = (double*)malloc(sizeof(double)*M);
		} else if (flag == 2) { // IMU + Sonar update
			M = imuSize+1;
			H = (double*)malloc(sizeof(double)*N*M);
			Kt = (double*)malloc(sizeof(double)*N*M);
			zbt = (double*)malloc(sizeof(double)*M);
		} else if (flag == 3) { // IMU + GPS + Sonar update
			M = imuSize+gpsSize+1;
			H = (double*)malloc(sizeof(double)*N*M);
			Kt = (double*)malloc(sizeof(double)*N*M);
			zbt = (double*)malloc(sizeof(double)*M);
		}

		// 1. Get current state, get mu
		getCurrentState(xcurr);

		// 2. Propogate the state, compute mubt
		runProcessModel(xcurr, mubt, dt);

		// 3. Compute process Jacobian, compute G
		computeProcessJacobian(xcurr, G, dt);

		// 4. Propogate the covariance, compute Sbart
		computePredictedVariance(G, Sbart);

		// 5. Compute the expected measurements, compute zbt
		runMeasModel(mubt, zbt, flag);

		// 6. Compute the measurement Jacobian, compute H
		computeMeasJacobian(mubt, H, flag);

		// 7. Calculate the Kalman gain, compute K
		computeKalmanGain(Sbart, H, Kt, flag);

		// 8. Update the state, compute mu
		// 9. Update the covariance, compute Sigma
		computeNewMeanandCovariance(mubt, Kt, zbt, Sbart, H, flag);

		// Print state
		printData(com.tIMU[1]);

		free(xcurr); free(G); free(mubt); free(Sbart);
		free(Kt); free(H); free(zbt);
	}
}


/**********************************
Initialize the current state
**********************************/
void EKF::initializeState() {
	
	// Initial heading from magnetometer, assuming 0 pitch and roll
	double psi0 = atan2(apdat.my,apdat.mx)-atan2(E_Mag_Field,N_Mag_Field);
	
	// Initialize state
	sv.Np  = apdat.GPS_N;
	sv.Ep  = apdat.GPS_E;
	sv.Dp  = apdat.GPS_D;
	sv.VNi = apdat.GPS_VN;
	sv.VEi = apdat.GPS_VE;
	sv.VDi = 0.0;
	sv.q0 = cos(0.5*psi0);
	sv.q1 = 0.0;
	sv.q2 = 0.0;
	sv.q3 = sin(0.5*psi0);
	pv.VNi_wind = 0.0;
	pv.VEi_wind = 0.0;
	pv.wbx = apdat.p;
	pv.wby = apdat.q;
	pv.wbz = apdat.r;
	pv.barob = -apdat.baropress + 101325.0*pow((1.0+0.0000225721785*apdat.GPS_D),5.256);
	pv.abx = 0.0;
	pv.aby = 0.0;
	pv.abz = 0.0;
	sv.p = 0.0;
	sv.q = 0.0;
	sv.r = 0.0;
}

/**********************************
Get the current state
**********************************/
void EKF::getCurrentState(double *xcurr) {
	*xcurr      = sv.Np;
	*(xcurr+1)  = sv.Ep;
	*(xcurr+2)  = sv.Dp;
	*(xcurr+3)  = sv.VNi;
	*(xcurr+4)  = sv.VEi;
	*(xcurr+5)  = sv.VDi;
	*(xcurr+6)  = sv.q0;
	*(xcurr+7)  = sv.q1;
	*(xcurr+8)  = sv.q2;
	*(xcurr+9)  = sv.q3;
	*(xcurr+10) = pv.VNi_wind;
	*(xcurr+11) = pv.VEi_wind;
	*(xcurr+12) = pv.wbx;
	*(xcurr+13) = pv.wby;
	*(xcurr+14) = pv.wbz;
	*(xcurr+15) = pv.barob;
	*(xcurr+16) = pv.abx;
	*(xcurr+17) = pv.aby;
	*(xcurr+18) = pv.abz;
}


/**********************************
Run the kinematic process model
**********************************/
void EKF::runProcessModel(double *x, double *mubt, double dt) {

	// Extract state
	double Np = *(x+0);
	double Ep = *(x+1);
	double Dp = *(x+2);
	double VNi = *(x+3);
	double VEi = *(x+4);
	double VDi = *(x+5);
	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double VNw = *(x+10);
	double VEw = *(x+11);
	double wbx = *(x+12);
	double wby = *(x+13);
	double wbz = *(x+14);
	double barob = *(x+15);
	double abx = *(x+16);
	double aby = *(x+17);
	double abz = *(x+18);
	
	// Extract measurements
	double ax = apdat.ax1;
	double ay = apdat.ay1;
	double az = apdat.az1;
	double wx = apdat.p;
	double wy = apdat.q;
	double wz = apdat.r;

	// Constants
	double g = 9.81; // Acceleration due to gravity [m/s^2]

	// Position propogation
	*(mubt+0) = Np + VNi*dt;
	*(mubt+1) = Ep + VEi*dt;
	*(mubt+2) = Dp + VDi*dt;

	// Velocity propogation
	double Tb2i[9];		// Rotation matrix from body frame to inertial frame
	// In the form [ 0 1 2 ; 3 4 5 ; 6 7 8];

	Tb2i[0] = 1.0-2.0*q2*q2-2.0*q3*q3;
	Tb2i[1] = 2.0*q1*q2-2.0*q0*q3;
	Tb2i[2] = 2.0*q1*q3+2.0*q0*q2;
	Tb2i[3] = 2.0*q1*q2+2.0*q0*q3;
	Tb2i[4] = 1.0-2.0*q1*q1-2.0*q3*q3;
	Tb2i[5] = 2.0*q2*q3-2.0*q0*q1;
	Tb2i[6] = 2.0*q1*q3-2.0*q0*q2;
	Tb2i[7] = 2.0*q2*q3+2.0*q0*q1;
	Tb2i[8] = 1.0-2.0*q1*q1-2.0*q2*q2;

	*(mubt+3) = VNi - (Tb2i[0]*(ax-abx) + Tb2i[1]*(ay-aby) + Tb2i[2]*(az-abz))*dt;
	*(mubt+4) = VEi - (Tb2i[3]*(ax-abx) + Tb2i[4]*(ay-aby) + Tb2i[5]*(az-abz))*dt;
	*(mubt+5) = VDi - (Tb2i[6]*(ax-abx) + Tb2i[7]*(ay-aby) + Tb2i[8]*(az-abz))*dt + g*dt;

	// Calculate delta euler angles taking sensor misalignment into account
	double dPhi   = (wx-wbx)*dt;
	double dTheta = (wy-wby)*dt;
	double dPsi   = (wz-wbz)*dt;
	double s = 0.5*sqrt(dPhi*dPhi + dTheta*dTheta + dPsi*dPsi);
	double coss = cos(s);
	double sins_over_s;
	if (s == 0.0) {
		sins_over_s = 1.0;
	} else {
		sins_over_s = sin(s)/s;
	}

	// Calculate new quaternions
	double qnew[4];
	qnew[0] = coss*q0 - 0.5*sins_over_s*(dPhi*q1 + dTheta*q2 + dPsi*q3);
	qnew[1] = coss*q1 - 0.5*sins_over_s*(-dPhi*q0 - dPsi*q2 + dTheta*q3);
	qnew[2] = coss*q2 - 0.5*sins_over_s*(-dTheta*q0 + dPsi*q1 - dPhi*q3);
	qnew[3] = coss*q3 - 0.5*sins_over_s*(-dPsi*q0 - dTheta*q1 + dPhi*q2);

	// Normalize quaternions
	double invqmag = 1.0/sqrt(qnew[0]*qnew[0] + qnew[1]*qnew[1] + qnew[2]*qnew[2] + qnew[3]*qnew[3]);
	*(mubt+6) = qnew[0]*invqmag;
	*(mubt+7) = qnew[1]*invqmag;
	*(mubt+8) = qnew[2]*invqmag;
	*(mubt+9) = qnew[3]*invqmag;

	// Parameter estimates don't change
	*(mubt+10) = VNw;
	*(mubt+11) = VEw;
	*(mubt+12) = wbx;
	*(mubt+13) = wby;
	*(mubt+14) = wbz;
	*(mubt+15) = barob;
	*(mubt+16) = abx;
	*(mubt+17) = aby;
	*(mubt+18) = abz;
}


/**********************************
Compute process model Jacobian
**********************************/
void EKF::computeProcessJacobian(double *x, double *G, double dt) {

	// i = function
	// j = state

	// G is in FORTRAN row major format
	// G(i,j) = *(G+j+i*numStates)

	// Initialize G to zeros
	for (int i = 0; i < numStates; i++) {
		for (int j = 0; j < numStates; j++) {
			*(G+j+i*numStates) = 0.0;
		}
	}

	// Extract state
	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double abx = *(x+16);
	double aby = *(x+17);
	double abz = *(x+18);

	// Extract measurements ("controls")
	double ax = apdat.ax1;
	double ay = apdat.ay1;
	double az = apdat.az1;
	double wx = apdat.p;
	double wy = apdat.q;
	double wz = apdat.r;

	// Position jacobian elements
	*(G+0+0*numStates) = 1.0; 	// dN/dN
	*(G+3+0*numStates) = dt;	// dN/dVN

	*(G+1+1*numStates) = 1.0;	// dE/dE
	*(G+4+1*numStates) = dt;	// dE/dVE

	*(G+2+2*numStates) = 1.0;	// dD/dD
	*(G+5+2*numStates) = dt;	// dD/dVD

	// Velocity Jacobian elements
	*(G+3+3*numStates) = 1.0; // dVN/dVn
	*(G+6+3*numStates) = -2.0*dt*(-q3*ay+q3*aby+q2*az-q2*abz); // dVN/dq0
	*(G+7+3*numStates) = -2.0*dt*(q2*ay-q2*aby+q3*az-q3*abz); // dVN/dq1
	*(G+8+3*numStates) = 2.0*dt*(2.0*q2*ax-2.0*q2*abx-q1*ay+q1*aby-q0*az+q0*abz); // dVN/dq2
	*(G+9+3*numStates) = 2.0*dt*(2.0*q3*ax-2.0*q3*abx+q0*ay-q0*aby-q1*az+q1*abz); // dVN/dq3
	*(G+16+3*numStates) = -dt*(-1.0+2.0*q2*q2+2.0*q3*q3); // dVN/dabx
	*(G+17+3*numStates) = 2.0*dt*(q1*q2-q0*q3); // dVN/daby
	*(G+18+3*numStates) = 2.0*dt*(q1*q3+q0*q2); // dVN/dabz
	
	*(G+4+4*numStates) = 1.0; // dVE/dVE
	*(G+6+4*numStates) = -2.0*dt*(q3*ax-q3*abx-q1*az+q1*abz); // dVE/dq0
	*(G+7+4*numStates) = -2.0*dt*(q2*ax-q2*abx-2.0*q1*ay+2.0*q1*aby-q0*az+q0*abz); // dVE/dq1
	*(G+8+4*numStates) = -2.0*dt*(q1*ax-q1*abx+q3*az-q3*abz); // dVE/dq2
	*(G+9+4*numStates) = -2.0*dt*(q0*ax-q0*abx-2.0*q3*ay+2.0*q3*aby+q2*az-q2*abz); // dVE/dq3
	*(G+16+4*numStates) = 2.0*dt*(q1*q2+q0*q3); // dVE/dabx
	*(G+17+4*numStates) = -dt*(-1.0+2.0*q1*q1+2.0*q3*q3); // dVE/daby
	*(G+18+4*numStates) = 2.0*dt*(q2*q3-q0*q1); // dVE/dabz
	
	*(G+5+5*numStates) = 1.0; // dVD/dVD
	*(G+6+5*numStates) = 2.0*dt*(q2*ax-q2*abx-q1*ay+q1*aby); // dVD/dq0
	*(G+7+5*numStates) = -2.0*dt*(q3*ax-q3*abx+q0*ay-q0*aby-2.0*q1*az+2.0*q1*abz); // dVD/dq1
	*(G+8+5*numStates) = 2.0*dt*(q0*ax-q0*abx-q3*ay+q3*aby+2.0*q2*az-2.0*q2*abz); // dVD/dq2
	*(G+9+5*numStates) = -2.0*dt*(q1*ax-q1*abx+q2*ay-q2*aby); // dVD/dq3
	*(G+16+5*numStates) = -2.0*dt*(-q1*q3+q0*q2); // dVD/dabx
	*(G+17+5*numStates) = 2.0*dt*(q2*q3+q0*q1); // dVD/daby
	*(G+18+5*numStates) = -dt*(-1.0+2.0*q1*q1+2.0*q2*q2); // dVD/dabz

	// Quaternion Jacobian elements (finite differencing!)
	double dx = 1e-8;
 	double *xp = (double*)malloc(sizeof(double)*numStates);
 	double *xm = (double*)malloc(sizeof(double)*numStates);

	// Quaternion sensitivity to quaterions
	for (int j = 6; j < 10; j++) {

		// Perturb state
		for (int k = 0; k < numStates; k++) {
			*(xp+k) = *(x+k);
			*(xm+k) = *(x+k);
		}
		*(xp+j) += dx;
		*(xm+j) -= dx;

		// Compute new xp quaternion
		quaternionUpdate(xp,wx,wy,wz,dt);

		// Compute new xm quaternion
		quaternionUpdate(xm,wx,wy,wz,dt);

		// Finite difference
		*(G+j+6*numStates) = (*(xp+6) - *(xm+6))/(2.0*dx);	// dq0/d..
		*(G+j+7*numStates) = (*(xp+7) - *(xm+7))/(2.0*dx);	// dq1/d..
		*(G+j+8*numStates) = (*(xp+8) - *(xm+8))/(2.0*dx);	// dq2/d..
		*(G+j+9*numStates) = (*(xp+9) - *(xm+9))/(2.0*dx);	// dq3/d..
	}

	// Quaternion sensitivity to gyro biases
	for (int j = 12; j < 15; j++) {

		// Perturb state
		for (int k = 0; k < numStates; k++) {
			*(xp+k) = *(x+k);
			*(xm+k) = *(x+k);
		}
		*(xp+j) += dx;
		*(xm+j) -= dx;

		// Compute new xp quaternion
		quaternionUpdate(xp,wx,wy,wz,dt);

		// Compute new xm quaternion
		quaternionUpdate(xm,wx,wy,wz,dt);

		// Finite difference
		*(G+j+6*numStates) = (*(xp+6) - *(xm+6))/(2.0*dx);	// dq0/d..
		*(G+j+7*numStates) = (*(xp+7) - *(xm+7))/(2.0*dx);	// dq1/d..
		*(G+j+8*numStates) = (*(xp+8) - *(xm+8))/(2.0*dx);	// dq2/d..
		*(G+j+9*numStates) = (*(xp+9) - *(xm+9))/(2.0*dx);	// dq3/d..
	}

	// Parameter Jacobian elements
	for (int i = 10; i < numStates; i++) {
		*(G+i+i*numStates) = 1.0;
	}

 	free(xp); free(xm);
}



/**********************************
Compute just the quaternion part
of the process model update
for finite differencing to get
process model Jacobian
**********************************/
void EKF::quaternionUpdate(double *x, double wx, double wy, double wz, double dt) {

	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double wbx = *(x+12);
	double wby = *(x+13);
	double wbz = *(x+14);

	double dPhi   = (wx-wbx)*dt;
	double dTheta = (wy-wby)*dt;
	double dPsi   = (wz-wbz)*dt;
	double s = 0.5*sqrt(dPhi*dPhi + dTheta*dTheta + dPsi*dPsi);
	double coss = cos(s);
	double sins_over_s;
	if (s == 0) {
		sins_over_s = 1.0;
	} else {
		sins_over_s = sin(s)/s;
	}

	// Calculate new quaternions
	double qnew[4];
	qnew[0] = coss*q0 - 0.5*sins_over_s*(dPhi*q1 + dTheta*q2 + dPsi*q3);
	qnew[1] = coss*q1 - 0.5*sins_over_s*(-dPhi*q0 - dPsi*q2 + dTheta*q3);
	qnew[2] = coss*q2 - 0.5*sins_over_s*(-dTheta*q0 + dPsi*q1 - dPhi*q3);
	qnew[3] = coss*q3 - 0.5*sins_over_s*(-dPsi*q0 - dTheta*q1 + dPhi*q2);

	// Normalize quaternions and put back in input vector
	double invqmag = 1.0/sqrt(qnew[0]*qnew[0] + qnew[1]*qnew[1] + qnew[2]*qnew[2] + qnew[3]*qnew[3]);
	*(x+6) = qnew[0]*invqmag;
	*(x+7) = qnew[1]*invqmag;
	*(x+8) = qnew[2]*invqmag;
	*(x+9) = qnew[3]*invqmag;
}



/**********************************
Update convariance estimate based
on process model Jacobian

Should be...
Input: G,P,R,dt
Output: Sbart
**********************************/
void EKF::computePredictedVariance(double *G, double *Sbart) {

	// Sbart = Gt*Stm1*Gt^T + Rt*dt
	double *tmp = (double*)malloc(sizeof(double)*numStates*numStates);
	double *Ptmp = (double*)malloc(sizeof(double)*numStates*numStates);

	int M = numStates;

	// Create local copy
	for (int i = 0; i < numStates*numStates; i++) {
		*(Ptmp+i) = *(P+i);
	}

	// Compute Gt*Stm1
	dgemm_("T","N",&M,&M,&M,&done,G,&M,Ptmp,&M,&dzero,tmp,&M);
	// Compute (Gt*Stm1)*Gt^T
	dgemm_("N","N",&M,&M,&M,&done,tmp,&M,G,&M,&dzero,Sbart,&M);
	// Add Rt
	for (int i = 0; i < numStates; i++) {
		*(Sbart+i+i*numStates) += R[i]*dt;
	}

	free(tmp); free(Ptmp);
}



/**********************************
Run measurement model - predict
measurements at input state
**********************************/
void EKF::runMeasModel(double *x, double *zbt, int flag) {

	// Extract state
	double Np = *(x+0);
	double Ep = *(x+1);
	double Dp = *(x+2);
	double VNi = *(x+3);
	double VEi = *(x+4);
	double VDi = *(x+5);
	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double VNw = *(x+10);
	double VEw = *(x+11);
	double barob = *(x+15);
    double phi, th;
	double Ti2b[9];		// Rotation matrix from inertial to body frame
	// In the form [ 0 1 2 ; 3 4 5 ; 6 7 8];
	Ti2b[0] = 1.0-2.0*q2*q2-2.0*q3*q3;
	Ti2b[1] = 2.0*q1*q2+2.0*q0*q3;
	Ti2b[2] = 2.0*q1*q3-2.0*q0*q2;
	Ti2b[3] = 2.0*q1*q2-2.0*q0*q3;
	Ti2b[4] = 1.0-2.0*q1*q1-2.0*q3*q3;
	Ti2b[5] = 2.0*q2*q3+2.0*q0*q1;
	Ti2b[6] = 2.0*q1*q3+2.0*q0*q2;
	Ti2b[7] = 2.0*q2*q3-2.0*q0*q1;
	Ti2b[8] = 1.0-2.0*q1*q1-2.0*q2*q2;


	*(zbt+0) = 101325.0*(pow(1.0+0.0000225721785*Dp,5.256))-barob;
	*(zbt+1) = 0.6125*((VNi-VNw)*(VNi-VNw)+(VEi-VEw)*(VEi-VEw)+VDi*VDi);
	*(zbt+2) = Ti2b[0]*N_Mag_Field + Ti2b[1]*E_Mag_Field + Ti2b[2]*D_Mag_Field;
	*(zbt+3) = Ti2b[3]*N_Mag_Field + Ti2b[4]*E_Mag_Field + Ti2b[5]*D_Mag_Field;
	*(zbt+4) = Ti2b[6]*N_Mag_Field + Ti2b[7]*E_Mag_Field + Ti2b[8]*D_Mag_Field;

	if (flag == 0) { // GPS update and pseudomeasurement updates too
		*(zbt+5)  = Np;
		*(zbt+6)  = Ep;
		*(zbt+7)  = Dp;
		*(zbt+8)  = VNi;
		*(zbt+9)  = VEi;
		*(zbt+10) = VDi;
		*(zbt+11) = Ti2b[0]*(VNi-VNw)+Ti2b[1]*(VEi-VEw)+Ti2b[2]*VDi - sqrt((VNi-VNw)*(VNi-VNw)+(VEi-VEw)*(VEi-VEw)+VDi*VDi)*cos(0.0872665); // 5 deg angle of attack assumed...
		*(zbt+12) = Ti2b[3]*(VNi-VNw)+Ti2b[4]*(VEi-VEw)+Ti2b[5]*VDi;
		*(zbt+13) = Ti2b[6]*(VNi-VNw)+Ti2b[7]*(VEi-VEw)+Ti2b[8]*VDi - sqrt((VNi-VNw)*(VNi-VNw)+(VEi-VEw)*(VEi-VEw)+VDi*VDi)*sin(0.0872665);	
	}

	if (flag == 2) { // IMU, Sonar
	    phi = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
	    th  = asin(2*(q0*q2-q3*q1));
	    *(zbt+5) = -(Dp+1.75/2.0*sin(phi))/(cos(phi)*cos(th));
	}

	if (flag == 3) { // IMU, GPS, Sonar
	    phi = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2));
	    th  = asin(2*(q0*q2-q3*q1));
	    *(zbt+14) = -(Dp+1.75/2.0*sin(phi))/(cos(phi)*cos(th));
	}
}



/**********************************
Compute Jacobian of measurement model
**********************************/
void EKF::computeMeasJacobian(double *x, double *H, int flag) {

	// i = function (N)
	// j = state (numstates)

	int N = 0;
	// H(i,j) = *(H+i+j*N)
	if (flag == 1) {
		N = imuSize;
	} else if (flag == 0) {
		N = imuSize+gpsSize;
	} else if (flag == 2) {
		N = imuSize + 1;
	} else if (flag == 3) {
		N = imuSize+gpsSize+1;
	}

	// Initialize Jacobian to zeros
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < numStates; j++) {
			*(H+i+j*N) = 0.0;
		}
	}

	// Extract state
	double Dp = *(x+2);
	double VNi = *(x+3);
	double VEi = *(x+4);
	double VDi = *(x+5);
	double q0 = *(x+6);
	double q1 = *(x+7);
	double q2 = *(x+8);
	double q3 = *(x+9);
	double VNw = *(x+10);
	double VEw = *(x+11);

	// Baro pressure Jacobian elements
	*(H+0+2*N) = 12.021134185109700*pow(1+0.0000225721785*Dp,4.256); 	// dbb/dD
	*(H+0+15*N) = -1.0;	// dbb/dbb

	// Differential pressure Jacobian elements
	*(H+1+3*N) = 1.225*(VNi-VNw);	// ddb/dVN
	*(H+1+4*N) = 1.225*(VEi-VEw);	// ddb/dVE
	*(H+1+5*N) = 1.225*VDi;			// ddb/dVD
	*(H+1+10*N) = -1.225*(VNi-VNw);	// ddb/dVWN
	*(H+1+11*N) = -1.225*(VEi-VEw);	// ddb/dVWE

	// Magnetometer Jacobian elements
	*(H+2+6*N) = 2*q3*E_Mag_Field - 2*q2*D_Mag_Field;						// dMX/dq0
	*(H+2+7*N) = 2*q2*E_Mag_Field + 2*q3*D_Mag_Field;						// dMX/dq1
	*(H+2+8*N) = -4*q2*N_Mag_Field + 2*q1*E_Mag_Field - 2*q0*D_Mag_Field;	// dMX/dq2
	*(H+2+9*N) = -4*q3*N_Mag_Field + 2*q0*E_Mag_Field + 2*q1*D_Mag_Field;	// dMX/dq3

	*(H+3+6*N) = -2*q3*N_Mag_Field + 2*q1*D_Mag_Field;						// dMY/dq0
	*(H+3+7*N) = 2*q2*N_Mag_Field - 4*q1*E_Mag_Field + 2*q0*D_Mag_Field;	// dMY/dq1
	*(H+3+8*N) = 2*q1*N_Mag_Field + 2*q3*D_Mag_Field;						// dMY/dq2
	*(H+3+9*N) = -2*q0*N_Mag_Field - 4*q3*E_Mag_Field + 2*q2*D_Mag_Field;	// dMY/dq3

	*(H+4+6*N) = 2*q2*N_Mag_Field - 2*q1*E_Mag_Field;						// dMZ/dq0
	*(H+4+7*N) = 2*q3*N_Mag_Field - 2*q0*E_Mag_Field - 4*q1*D_Mag_Field;	// dMZ/dq1
	*(H+4+8*N) = 2*q0*N_Mag_Field + 2*q3*E_Mag_Field - 4*q2*D_Mag_Field;	// dMZ/dq2
	*(H+4+9*N) = 2*q1*N_Mag_Field + 2*q2*E_Mag_Field;						// dMZ/dq3
	
	// GPS Jacobian elements
	if (flag == 0) {
		*(H+5+0*N)  = 1.0;
		*(H+6+1*N)  = 1.0;
		*(H+7+2*N)  = 1.0;
		*(H+8+3*N)  = 1.0;
		*(H+9+4*N)  = 1.0;
		*(H+10+5*N) = 1.0;
		
		double calpha = cos(0.0872665);
    	double salpha = sin(0.0872665);
    	double airspeed = sqrt((VNi-VNw)*(VNi-VNw)+(VEi-VEw)*(VEi-VEw)+VDi*VDi);
		*(H+11+3*N) = 1.0-2.0*q2*q2-2.0*q3*q3 - calpha*(VNi-VNw)/airspeed; // dubody/dVN
		*(H+11+4*N) = 2.0*q1*q2+2.0*q0*q3 - calpha*(VEi-VEw)/airspeed; // dubody/dVE
		*(H+11+5*N) = 2.0*q1*q3-2.0*q0*q2 - calpha*VDi/airspeed; // dubody/dVD
		*(H+11+6*N) = 2.0*q3*VEi-2.0*q3*VEw-2.0*q2*VDi; // dubody/dq0
		*(H+11+7*N) = 2.0*q2*VEi-2.0*q2*VEw+2.0*q3*VDi; // dubody/dq1
		*(H+11+8*N) = -4.0*q2*VNi+4.0*q2*VNw+2.0*q1*VEi-2.0*q1*VEw-2.0*q0*VDi; // dubody/dq2
		*(H+11+9*N) = -4.0*q3*VNi+4.0*q3*VNw+2.0*q0*VEi-2.0*q0*VEw+2.0*q1*VDi; // dubody/dq3
		*(H+11+10*N) = -1.0+2.0*q2*q2+2.0*q3*q3 + calpha*(VNi-VNw)/airspeed; // dubody/dVNw
		*(H+11+11*N) = -2.0*q1*q2-2.0*q0*q3 + calpha*(VEi-VEw)/airspeed; // dubody/dVEw

		*(H+12+3*N) = 2.0*q1*q2-2.0*q0*q3; // dvbody/dVN
		*(H+12+4*N) = 1.0-2.0*q1*q1-2.0*q3*q3; // dvbody/dVE
		*(H+12+5*N) = 2.0*q2*q3+2.0*q0*q1; // dvbody/dVD
		*(H+12+6*N) = -2.0*q3*(VNi-VNw)+2.0*q1*VDi; // dvbody/dq0
		*(H+12+7*N) = 2.0*q2*(VNi-VNw)-4.0*q1*(VEi-VEw)+2.0*q0*VDi; // dvbody/dq1
		*(H+12+8*N) = 2.0*q1*(VNi-VNw)+2.0*q3*VDi; // dvbody/dq2
		*(H+12+9*N) = -2.0*q0*(VNi-VNw)-4.0*q3*(VEi-VEw)+2.0*q2*VDi; // dvbody/dq3
		*(H+12+10*N) = -2.0*q1*q2+2.0*q0*q3; // dvbody/dVNw
		*(H+12+11*N) = -1.0+2.0*q1*q1+2.0*q3*q3; // dvbody/dVEw
				
		*(H+13+3*N) = 2.0*q1*q3+2.0*q0*q2 - salpha*(VNi-VNw)/airspeed; // dwbody/dVN
		*(H+13+4*N) = 2.0*q2*q3-2.0*q0*q1 - salpha*(VEi-VEw)/airspeed; // dwbody/dVE
		*(H+13+5*N) = 1.0-2.0*q1*q1-2.0*q2*q2 - salpha*VDi/airspeed; // dwbody/dVD
		*(H+13+6*N) = 2.0*q2*(VNi-VNw)-2.0*q1*(VEi-VEw); // dwbody/dq0
		*(H+13+7*N) = 2.0*q3*(VNi-VNw)-2.0*q0*(VEi-VEw)-4.0*q1*VDi; // dwbody/dq1
		*(H+13+8*N) = 2.0*q0*(VNi-VNw)+2.0*q3*(VEi-VEw)-4.0*q2*VDi; // dwbody/dq2
		*(H+13+9*N) = 2.0*q1*(VNi-VNw)+2.0*q2*(VEi-VEw); // dwbody/dq3
		*(H+13+10*N) = -2.0*q1*q3-2.0*q0*q2 + salpha*(VNi-VNw)/airspeed; // dwbody/dVNw
		*(H+13+11*N) = -2.0*q2*q3+2.0*q0*q1 + salpha*(VEi-VEw)/airspeed; // dwbody/dVEw
	}

	if (flag == 2) {
	    *(H+5+2*N) = -1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)));
	    *(H+5+6*N) = (((q1*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(7.0/4.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q1*1.0/pow(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0,3.0/2.0)*pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,3.0)*(7.0/4.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0))/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))-(q2*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/pow(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0,3.0/2.0)*(q0*q2*2.0-q1*q3*2.0)*2.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(q1*cos(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))*1.0/pow(sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0))),2.0)*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)*2.0)/((pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*((q1*q1)*2.0+(q2*q2)*2.0-1.0));
	    *(H+5+7*N) = (1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)*((q0*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(7.0/4.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)+(1.0/pow(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0,3.0/2.0)*(q1*pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,3.0)*5.6E1-q0*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*2.8E1)*(q0*q1*2.0+q2*q3*2.0)*(1.0/1.6E1))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q1*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*(7.0/2.0)))/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(q3*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/pow(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0,3.0/2.0)*(q0*q2*2.0-q1*q3*2.0)*2.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(cos(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))*1.0/pow(sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0))),2.0)*((q0*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q1*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*4.0)*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0))/(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0);
	    *(H+5+8*N) = (1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)*((q3*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(7.0/4.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)+(1.0/pow(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0,3.0/2.0)*(q2*pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,3.0)*5.6E1-q3*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*2.8E1)*(q0*q1*2.0+q2*q3*2.0)*(1.0/1.6E1))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q2*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*(7.0/2.0)))/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))-(q0*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/pow(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0,3.0/2.0)*(q0*q2*2.0-q1*q3*2.0)*2.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(cos(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))*1.0/pow(sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0))),2.0)*((q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q2*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*4.0)*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0))/(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0);
	    *(H+5+9*N) = (((q2*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(7.0/4.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q2*1.0/pow(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0,3.0/2.0)*pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,3.0)*(7.0/4.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0))/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(q1*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/pow(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0,3.0/2.0)*(q0*q2*2.0-q1*q3*2.0)*2.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(q2*cos(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))*1.0/pow(sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0))),2.0)*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)*2.0)/((pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*((q1*q1)*2.0+(q2*q2)*2.0-1.0));
	}

	if (flag == 3) {
	    *(H+14+2*N) = -1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)));
	    *(H+14+6*N) = (((q1*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(7.0/4.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q1*1.0/pow(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0,3.0/2.0)*pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,3.0)*(7.0/4.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0))/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))-(q2*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/pow(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0,3.0/2.0)*(q0*q2*2.0-q1*q3*2.0)*2.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(q1*cos(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))*1.0/pow(sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0))),2.0)*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)*2.0)/((pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*((q1*q1)*2.0+(q2*q2)*2.0-1.0));
	    *(H+14+7*N) = (1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)*((q0*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(7.0/4.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)+(1.0/pow(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0,3.0/2.0)*(q1*pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,3.0)*5.6E1-q0*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*2.8E1)*(q0*q1*2.0+q2*q3*2.0)*(1.0/1.6E1))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q1*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*(7.0/2.0)))/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(q3*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/pow(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0,3.0/2.0)*(q0*q2*2.0-q1*q3*2.0)*2.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(cos(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))*1.0/pow(sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0))),2.0)*((q0*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q1*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*4.0)*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0))/(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0);
	    *(H+14+8*N) = (1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)*((q3*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(7.0/4.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)+(1.0/pow(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0,3.0/2.0)*(q2*pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,3.0)*5.6E1-q3*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*2.8E1)*(q0*q1*2.0+q2*q3*2.0)*(1.0/1.6E1))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q2*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*(7.0/2.0)))/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))-(q0*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/pow(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0,3.0/2.0)*(q0*q2*2.0-q1*q3*2.0)*2.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(cos(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))*1.0/pow(sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0))),2.0)*((q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q2*(q0*q1*2.0+q2*q3*2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)*4.0)*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0))/(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0);
	    *(H+14+9*N) = (((q2*1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(7.0/4.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0)-q2*1.0/pow(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0,3.0/2.0)*pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,3.0)*(7.0/4.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0))/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(q1*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/pow(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0,3.0/2.0)*(q0*q2*2.0-q1*q3*2.0)*2.0)/sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))+(q2*cos(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0)))*1.0/pow(sin(3.141592653589793*(1.0/2.0)+atan((q0*q1*2.0+q2*q3*2.0)/((q1*q1)*2.0+(q2*q2)*2.0-1.0))),2.0)*(Dp-(1.0/sqrt(pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*(q0*q1*1.4E1+q2*q3*1.4E1)*(1.0/8.0))/((q1*q1)*2.0+(q2*q2)*2.0-1.0))*1.0/sqrt(-pow(q0*q2*2.0-q1*q3*2.0,2.0)+1.0)*2.0)/((pow(q0*q1*2.0+q2*q3*2.0,2.0)*1.0/pow((q1*q1)*2.0+(q2*q2)*2.0-1.0,2.0)+1.0)*((q1*q1)*2.0+(q2*q2)*2.0-1.0));
	}
}




/**********************************
Compute the Kalman gain
**********************************/
void EKF::computeKalmanGain(double *Sbart, double *H, double *Kt, int flag) {

	int N, M, info;
	N = numStates;

	if (flag == 1) { // IMU
		M = imuSize;
	} else if (flag == 0) { // GPS+IMU
		M = gpsSize + imuSize;
	} else if (flag == 2) { // IMU+Sonar
		M = imuSize + 1;
	} else if (flag == 3) { // IMU+GPS+Sonar
		M = gpsSize + imuSize + 1;
	}

	int *ipiv = (int*)malloc(sizeof(double)*numStates);
	double *B = (double*)malloc(sizeof(double)*numStates*M);
	double *A = (double*)malloc(sizeof(double)*numStates*M);
	double *tA  = (double*)malloc(sizeof(double)*numStates*numStates);
	double *tB  = (double*)malloc(sizeof(double)*numStates*numStates);

	// Kt = Sbart*Ht^T*(Ht*Sbart*Ht^T+Qt)^-1

	//	// Compute Sbart*Ht^T
	dgemm_("N","T",&N,&M,&N,&done,Sbart,&N,H,&M,&dzero,B,&N);

	// Compute Ht*Sbart*Ht^T
	dgemm_("N","N",&M,&M,&N,&done,H,&M,B,&N,&dzero,A,&M);

	// Ht*Sbart*Ht^T + Qt
	for (int i = 0; i < M; i++) {
		if (flag == 1) {
			*(A+i+i*M)+=Q_IMU[i];
		} else if (flag == 0) {
			*(A+i+i*M)+=Q_IMU_GPS[i];
		} else if (flag == 2) {
			*(A+i+i*M)+=Q_IMU_Sonar[i];
		} else if (flag == 3) {
			*(A+i+i*M)+=Q_IMU_GPS_Sonar[i];
		}
	}

	// Compute transpose of A and B
	for (int i = 0; i < M; i++) {
		for (int j = 0; j < M; j++) {
			*(tA+i+j*M) = *(A+j+i*M);
		}
	}
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < M; j++) {
			*(tB+j+i*M) = *(B+i+j*N);
		}
	}

	// Matrix solve
	dgesv_(&M, &N, tA, &M, ipiv, tB, &M, &info);


	for (int i = 0; i < M; i++) {
		for (int j = 0; j < N; j++) {
			*(Kt+j+i*N) = *(tB+i+j*M);
		}
	}
	free(A); free(B); free(tA); free(tB); free(ipiv);
}



/**********************************
Update state and covariance estimates
**********************************/
void EKF::computeNewMeanandCovariance(double *mubt, double *Kt, double *zbt,
		double *Sbart, double *H, int flag) {

	int N, M;
	N = numStates;
	double dzero = 0.0;
	double dmone = -1.0;

	if (flag == 1) { // IMU
		M = imuSize;
	} else if (flag == 0) { // GPS+IMU
		M = gpsSize + imuSize;
	} else if (flag == 2) { // IMU+Sonar
		M = imuSize + 1;
	} else if (flag == 3) { // IMU+GPS+Sonar
		M = gpsSize + imuSize + 1;
	}

	double *Ktz = (double*)malloc(sizeof(double)*N);
	double *zt  = (double*)malloc(sizeof(double)*M);
	double *mut = (double*)malloc(sizeof(double)*N);
	double *tmp = (double*)malloc(sizeof(double)*M);
	double *eyeN = (double*)malloc(sizeof(double)*N*N);

	// Pressure and magnetometer measurements
	*(zt+0) = apdat.baropress;
	*(zt+1) = apdat.diffpress;
	*(zt+2) = apdat.mx;
	*(zt+3) = apdat.my;
	*(zt+4) = apdat.mz;
	if (flag == 0) {	// GPS measurements and pseudomeasurements too
		*(zt+5) = apdat.GPS_N;
		*(zt+6) = apdat.GPS_E;
		*(zt+7) = apdat.GPS_D;
		*(zt+8) = apdat.GPS_VN;
		*(zt+9) = apdat.GPS_VE;
		*(zt+10) = apdat.GPS_VD;
		*(zt+11) = 0.0;
		*(zt+12) = 0.0;
		*(zt+13) = 0.0;
	}
	if (flag == 2) {
		*(zt+5) = apdat.sonar_range;
	} else if (flag == 3) {
		*(zt+14) = apdat.sonar_range;
	}


	// Write identity matrix
	for (int i = 0; i < numStates; i++) {
		for (int j = 0; j < numStates; j++) {
			*(eyeN+i+j*N) = 0.0;
			if (i == j) {
				*(eyeN+i+j*N) = 1.0;
			}
		}
	}

	// Real measurement minus predicted measurement
	for (int i = 0; i < M; i++) {
		*(tmp+i) = (*(zt+i)-*(zbt+i));
	}

	// Kt*(zt-zbt)
	dgemv_("N", &N, &M, &done, Kt, &N, tmp, &ione, &dzero, Ktz, &ione);

	// Compute new estimate
	for (int i = 0; i < numStates; i++) {
		*(mut+i) = *(mubt+i) + *(Ktz+i);
	}

	// Normalize quaternions
	double quatnorm = 1.0/sqrt(*(mut+6)*(*(mut+6))+*(mut+7)*(*(mut+7))+
			*(mut+8)*(*(mut+8))+*(mut+9)*(*(mut+9)));
	*(mut+6) *= quatnorm;
	*(mut+7) *= quatnorm;
	*(mut+8) *= quatnorm;
	*(mut+9) *= quatnorm;

	// Put new state estimate into the global variables
	updateState(mut);

	// (I-Kt*H)
	dgemm_("N","N", &N, &N, &M, &dmone, Kt, &N, H, &M, &done, eyeN, &N);

	// (I-Kt*H)*Sbart
	dgemm_("N","N", &N, &N, &N, &done, eyeN, &N, Sbart, &N, &dzero, P, &N);

	free(Ktz); free(zt); free(mut); free(tmp); free(eyeN);
}




/**********************************
Save state to global variable
**********************************/
void EKF::updateState(double *mubt) {
	if (*(mubt+0) > 1e6) {
		sv.Np = 1e6;
	} else if (*(mubt+0) < -1e6) {
		sv.Np = -1e6;
	} else {
		sv.Np = *(mubt+0);
	}
	if (*(mubt+1) > 1e6) {
		sv.Ep = 1e6;
	} else if (*(mubt+1) < -1e6) {
		sv.Ep = -1e6;
	} else {
		sv.Ep = *(mubt+1);
	}
	if (*(mubt+2) > 1e6) {
		sv.Dp = 1e6;
	} else if (*(mubt+2) < -1e6) {
		sv.Dp = -1e6;
	} else {
		sv.Dp = *(mubt+2);
	}
	if (*(mubt+3) > 50) {
		sv.VNi = 50;
	} else if (*(mubt+3) < -50) {
		sv.VNi = -50;
	} else {
		sv.VNi = *(mubt+3);
	}
	if (*(mubt+4) > 50) {
		sv.VEi = 50;
	} else if (*(mubt+4) < -50) {
		sv.VEi = -50;
	} else {
		sv.VEi = *(mubt+4);
	}
	if (*(mubt+5) > 50) {
		sv.VDi = 50;
	} else if (*(mubt+5) < -50) {
		sv.VDi = -50;
	} else {
		sv.VDi = *(mubt+5);
	}
	if (*(mubt+6) > 1) {
		sv.q0 = 1;
	} else if (*(mubt+6) < -1) {
		sv.q0 = -1;
	} else {
		sv.q0 = *(mubt+6);
	}
	if (*(mubt+7) > 1) {
		sv.q1 = 1;
	} else if (*(mubt+7) < -1) {
		sv.q1 = -1;
	} else {
		sv.q1 = *(mubt+7);
	}
	if (*(mubt+8) > 1) {
		sv.q2 = 1;
	} else if (*(mubt+8) < -1) {
		sv.q2 = -1;
	} else {
		sv.q2 = *(mubt+8);
	}
	if (*(mubt+9) > 1) {
		sv.q3 = 1;
	} else if (*(mubt+9) < -1) {
		sv.q3 = -1;
	} else {
		sv.q3 = *(mubt+9);
	}
	if (apdat.p > 20) {
		sv.p = 20;
	} else if (apdat.p < -20) {
		sv.p = -20;
	} else {
		sv.p = apdat.p;
	}
	if (apdat.q > 20) {
		sv.q = 20;
	} else if (apdat.q < -20) {
		sv.q = -20;
	} else {
		sv.q = apdat.q;
	}
	if (apdat.r > 20) {
		sv.r = 20;
	} else if (apdat.r < -20) {
		sv.r = -20;
	} else {
		sv.r = apdat.r;
	}
	if (*(mubt+10) > 50) {
		pv.VNi_wind = 50;
	} else if (*(mubt+10) < -50) {
		pv.VNi_wind = -50;
	} else {
		pv.VNi_wind = *(mubt+10);
	}
	if (*(mubt+11) > 50) {
		pv.VEi_wind = 50;
	} else if (*(mubt+11) < -50) {
		pv.VEi_wind = -50;
	} else {
		pv.VEi_wind = *(mubt+11);
	}
	if (*(mubt+12) > 10) {
		pv.wbx = 10;
	} else if (*(mubt+12) < -10) {
		pv.wbx = -10;
	} else {
		pv.wbx = *(mubt+12);
	}
	if (*(mubt+13) > 10) {
		pv.wby = 10;
	} else if (*(mubt+13) < -10) {
		pv.wby = -10;
	} else {
		pv.wby = *(mubt+13);
	}
	if (*(mubt+14) > 10) {
		pv.wbz = 10;
	} else if (*(mubt+14) < -10) {
		pv.wbz = -10;
	} else {
		pv.wbz = *(mubt+14);
	}
	if (*(mubt+15) > 10000) {
		pv.barob = 10000;
	} else if (*(mubt+15) < -10000) {
		pv.barob = -10000;
	} else {
		pv.barob = *(mubt+15);
	}
	if (*(mubt+16) > 10) {
		pv.abx = 10;
	} else if (*(mubt+16) < -10) {
		pv.abx = -10;
	} else {
		pv.abx = *(mubt+16);
	}
	if (*(mubt+17) > 10) {
		pv.aby = 10;
	} else if (*(mubt+17) < -10) {
		pv.aby = -10;
	} else {
		pv.aby = *(mubt+17);
	}
	if (*(mubt+18) > 10) {
		pv.abz = 10;
	} else if (*(mubt+18) < -10) {
		pv.abz = -10;
	} else {
		pv.abz = *(mubt+18);
	}
	
	// Compute Euler angles
	sv.phi = atan2(2.0*(sv.q0*sv.q1+sv.q2*sv.q3),1.0-2.0*(sv.q1*sv.q1+sv.q2*sv.q2));
	sv.theta = asin(2.0*(sv.q0*sv.q2-sv.q3*sv.q1));
	sv.psi = atan2(2.0*(sv.q0*sv.q3+sv.q1*sv.q2),1.0-2.0*(sv.q2*sv.q2+sv.q3*sv.q3));
}




/**********************************
Write data to log file
**********************************/
void EKF::printData(double t) {
	if (!pv.killMariner) {
		fw->updateValue("t",t);
		fw->updateValue("Np",sv.Np);
		fw->updateValue("Ep",sv.Ep);
		fw->updateValue("Dp",sv.Dp);
		fw->updateValue("VNi",sv.VNi);
		fw->updateValue("VEi",sv.VEi);
		fw->updateValue("VDi",sv.VDi);
		fw->updateValue("q0",sv.q0);
		fw->updateValue("q1",sv.q1);
		fw->updateValue("q2",sv.q2);
		fw->updateValue("q3",sv.q3);
		fw->updateValue("VNi_wind",pv.VNi_wind);
		fw->updateValue("VEi_wind",pv.VEi_wind);
		fw->updateValue("wbx",pv.wbx);
		fw->updateValue("wby",pv.wby);
		fw->updateValue("wbz",pv.wbz);
		fw->updateValue("barob",pv.barob);
		fw->updateValue("diffb",pv.diffb);
		fw->updateValue("abx",pv.abx);
		fw->updateValue("aby",pv.aby);
		fw->updateValue("abz",pv.abz);

		// Print line when EKF updates
		fw->printLine();
	} else {
		fw->closeFile();
	}
}

/**********************************
Destructor - do not change
**********************************/
EKF::~EKF() {
}
